/*
 * motorHAL.c
 *
 *  Created on: 15-12-2012
 *      Author: Maciej
 */

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_ints.h"
#include "inc/hw_timer.h"

#include "driverlib/debug.h"
#include "driverlib/fpu.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/rom.h"
#include "driverlib/sysctl.h"
#include "driverlib/i2c.h"
#include "utils/uartstdio.h"
#include "driverlib/rom_map.h"
#include "driverlib/timer.h"
#include "GY80.h"
#include "my_i2c.h"
#include "kalman.h"
#include "PID.h"
#include "math.h"

#define MOTOR_DUTY_MAX 1000

unsigned int motor_dead_zone = 680;

void motor_init()
    {

	//
	// init pwm timer
	// Configure PB2 as T3CCP0
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	GPIOPinConfigure(GPIO_PB2_T3CCP0);
	GPIOPinTypeTimer(GPIO_PORTB_BASE, GPIO_PIN_2);
	// Configure timer
	SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER3);
	TimerConfigure(TIMER3_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PWM);
	HWREG(TIMER3_BASE + TIMER_O_CTL) |= TIMER_CTL_TAPWML;
	TimerLoadSet(TIMER3_BASE, TIMER_A, MOTOR_DUTY_MAX+1);
	TimerMatchSet(TIMER3_BASE, TIMER_A, 0);
	TimerEnable(TIMER3_BASE, TIMER_A);

	// Set pin as output (direction)
	//ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_7 | GPIO_PIN_6);
	ROM_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_7 | GPIO_PIN_6, 0x00);
    }

void motor_set(float st)
    {
	ROM_GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_7 | GPIO_PIN_6, (st < 0 ? GPIO_PIN_6 : GPIO_PIN_7));

	if (st != 0)
	    {
		if(st<0) st=-st;

		st += motor_dead_zone;

		if (st > MOTOR_DUTY_MAX) st = MOTOR_DUTY_MAX; // saturation
	    }

	TimerMatchSet(TIMER3_BASE, TIMER_A, (int) st); //MOTOR_DUTY_MAX-
    }

